﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace cs5643.rigidbody
{
    public struct Trafo2D
    {
        private double[] data;

        public double this[int i, int j]
        {
            get { return data[i * 3 + j]; }
            set { data[i * 3 + j] = value; }
        }

        public static Trafo2D Identity()
        {
            return new Trafo2D(
                1, 0, 0, 
                0, 1, 0, 
                0, 0, 1);
        }

        public Trafo2D(
            double x00, double x01, double x02,
            double x10, double x11, double x12,
            double x20, double x21, double x22
            )
        {
            data = new double[9];
            data[0] = x00; data[1] = x01; data[2] = x02;
            data[3] = x10; data[4] = x11; data[5] = x12;
            data[6] = x20; data[7] = x21; data[8] = x22;
        }

        public Trafo2D Inverse
        {
            get
            {
                double d = Det;
                if (d == 0) throw new ArithmeticException("Singular Matrix");
                d = 1.0 / d;
                return Adjoint.Transposed.Times(d);
            }
        }

        private Trafo2D Times(double d)
        {
            return new Trafo2D(
                data[0] * d, data[1] * d, data[2] * d,
                data[3] * d, data[4] * d, data[5] * d,
                data[6] * d, data[7] * d, data[8] * d);
        }

        private double Det
        {
            get
            {
                return
                    data[0] * data[4] * data[8] + data[1] * data[5] * data[6] + data[2] * data[3] * data[7]
                  - data[6] * data[4] * data[2] - data[7] * data[5] * data[0] - data[8] * data[3] * data[1];
            }
        }

        private static double Det2x2(
            double x00, double x01,
            double x10, double x11
            )
        {
            return x00 * x11 - x10 * x01;
        }

        private Trafo2D Adjoint
        {
            get
            {
                return new Trafo2D(
                    Det2x2(
                        data[4], data[5],
                        data[7], data[8]), // 00
                    -Det2x2(
                        data[3], data[5],
                        data[6], data[8]), // 01
                    Det2x2(
                        data[3], data[4],
                        data[6], data[7]), // 02

                    -Det2x2(
                        data[1], data[2],
                        data[7], data[8]), // 10
                    Det2x2(
                        data[0], data[2],
                        data[6], data[8]), // 11
                    -Det2x2(
                        data[0], data[1],
                        data[6], data[7]), // 12

                    Det2x2(
                        data[1], data[2],
                        data[4], data[5]), // 20
                    -Det2x2(
                        data[0], data[2],
                        data[3], data[5]), // 21
                    Det2x2(
                        data[0], data[1],
                        data[3], data[4])  // 22

                    );
            }
        }

        private Trafo2D Transposed
        {
            get
            {
                return new Trafo2D(
                    data[0], data[3], data[6],
                    data[1], data[4], data[7],
                    data[2], data[5], data[8]
                    );
            }
        }
    }
}
